#!/usr/bin/python

import rospy

# import thread
from pi_driver.srv import (
    SetString,
    SetStringResponse,
    GetString,
    GetInt32,
    GetStringResponse,
    GetInt32Response,
)
from pi_driver.srv import SetInt32,SetInt32Response
from std_msgs.msg import String
import json
from balance_car import BrushlessDriver
from balance_car import BalanceCar

class BrushlessDriverNode:
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing......" % (self.node_name))
        self.driver = BrushlessDriver()
        self.car = BalanceCar(self.driver)
        rospy.Service("~set_throttle", SetInt32, self.srvSetThrottle)
        rospy.Service("~set_steering", SetInt32, self.srvSetSteering)
        self.srv_get_motor_state = rospy.Service(
            "~get_motor_state", GetString, self.cbGetMotorState
        )
        self.srv_set_motor_value = rospy.Service(
            "~set_motor_value", SetString, self.cbSetMotorValue
        )
        self.srv_set_balance_car = rospy.Service(
                    "~set_balance_car", SetString, self.cbSetBalanceCar
                )
        rospy.loginfo("[%s] Initialized......" % (self.node_name))

    def cbGetMotorState(self, params):
        port = params.data
        state = self.driver.state[port]
        return GetStringResponse(str(state))

    def cbSetMotorValue(self, params):
        value = json.loads(params.data)
        self.driver.send_cmd(value["port"], value["type"], value["value"])
        return SetStringResponse(params.data)

    def srvSetThrottle(self, params):
        self.car.state['throttle'] = params.value/100.0*self.car.max_throttle
        return SetInt32Response(params.port, params.value)

    def srvSetSteering(self, params):
        self.car.state['steering'] = params.value/100.0*self.car.max_steering
        return SetInt32Response(params.port, params.value)
    
    def cbSetBalanceCar(self,params):
        value = json.loads(params.data)
        if value["active"]:
            self.car.set_motor(value['L'],value['R'])
            self.car.set_active(True)
        else:
            self.car.set_active(False)
        return SetStringResponse(params.data)
    def onShutdown(self):
        rospy.loginfo("[%s] shutdown......" % (self.node_name))


if __name__ == "__main__":
    rospy.init_node("brushless_driver_node", anonymous=False)
    node = BrushlessDriverNode()
    rospy.on_shutdown(node.onShutdown)
    rospy.spin()
